#!/usr/bin/env python

# This example demonstrates the use of vtkCardinalSpline.
# It creates random points and connects them with a spline
import os, sys
import vtk
import vtktudoss
import cPickle
from wx import *
import wx.aui
from vtk.wx.wxVTKRenderWindow import *
from vtk.wx.wxVTKRenderWindowInteractor import *
from vtk.util.colors import tomato, banana, blue_light 
import numpy
import statistics
import time

import superthread
import AngleCalculations

try:
    import extended.Annotation as Annotation
    import extended.ColorScales as ColorScales
    import extended.BaseCalc as BaseCalc
    import extended.CoordinateSystems as CoordinateSystems
    import extended.GUI as GUI
    import extended.InverseKinematics as IK
    import extended.Filters as Filters
except ImportError:
    import Annotation
    import ColorScales
    import BaseCalc
    import CoordinateSystems
    import GUI
    import InverseKinematics as IK
    import Filters


[wxID_FRAME
] = [wx.NewId() for _init_ctrls in range(1)]
    
def create(parent, root, appdir):    
    return PatternViewer(parent, root, appdir)
    
class PatternViewer():

    pointwidgetinitialized = False
    
    BLmeshList = [	246.6	,	198.2	,	-1087.9	]	,\
                    [	238.8	,	222.9	,	-932.1	]	,\
                    [	244.4	,	321.6	,	-877	]	,\
                    [	244.5	,	371.7	,	-1071.1	]	,\
                    [	235.1	,	224	,	-928.9	]	,\
                    [	94.4	,	256.8	,	-924.4	]	,\
                    [	80.4	,	297.2	,	-897.3	]	,\
                    [	50.1	,	314.2	,	-914.2	]	,\
                    [	158.5	,	359.6	,	-942.8	]	,\
                    [	139.8	,	363.4	,	-1058.6	]	,\
                    [	54.4	,	270	,	-1215.2	]	,\
                    [	76.6	,	331.6	,	-1215.7	]	,\
                    [	122.8	,	206.9	,	-1454.8	]	,\
                    [	76.7	,	234.4	,	-1458	]	,\
                    [	250.9	,	224	,	-928.9	]	,\
                    [	389.1	,	260	,	-914.8	]	,\
                    [	405.8	,	301.4	,	-885.2	]	,\
                    [	431.3	,	323.5	,	-911.4	]	,\
                    [	321.9	,	365.2	,	-942.8	]	,\
                    [	355.2	,	363.4	,	-1056	]	,\
                    [	441.8	,	286.1	,	-1211.9	]	,\
                    [	396.4	,	328.2	,	-1211.5	]	,\
                    [	362.1	,	200.8	,	-1442.4	]	,\
                    [	407.5	,	237.8	,	-1444.8	]	,\
                    [	65.4	,	277.8	,	-932.9	]	,\
                    [	421.6	,	280.3	,	-923.8	]	

    imagenumber = 0
    
    def __init__(self, parent, root, appdir):
        self.parent = parent
        self._root = root
        self.appdir = appdir

        self.kdpoints = vtk.vtkPoints()
        self.kdvertices = vtk.vtkCellArray()
        self.kdpolydata = vtk.vtkPolyData()
        self.kdpoints2 = vtk.vtkPoints()
        self.kdvertices2 = vtk.vtkCellArray()        
        self.kdpolydata2 = vtk.vtkPolyData()
        self.kdtree_modified = True
        
        self.gui = GUI.create(self, self._root)                
        self.baseCalc = BaseCalc.create(self, self)
        self.colorscales = ColorScales.create(self)
        self.coordinatesystems = CoordinateSystems.create(self)        
        self.IK = IK.create(self)
        self.InitializePointWidget()
        
                
        self.shaderrate = 1.0        
        self.pulserate = 0.0
        self.CreateArmObject()
        self.AddSkeleton()
        
        self.angleCalculations = AngleCalculations.create(self, self._root)
        self.Filters = Filters.create(self)
        
        
        self.rollover_activated = False
        self.cellpickercreated = False      

        self.baseSkeleton = -1
        self.ActivateAngleRollOver(None)
        self.lbpe = self.gui.renwini3.AddObserver("LeftButtonPressEvent", \
                                                                            self.AngleRollOver, 1.0)
        
        
        #SPLINES
        self.splineCollection = []
        self.splineReferences = []
        self.splineCollection_seqfilter = []
        self.splineReferences_seqfilter = []
        
        self.Filters.UpdateShaderOpacity
        self.glyphlist = []
        
        self.statisticsActor = vtk.vtkActor()
        self.customPose = vtk.vtkActor()
        self.PoseActor9 = vtk.vtkActor()
                
        self.stablejointindex = 0 #0 = spine only, 1 = spine and clavicle etc.
        self.displayfrom = 0
        

        
    def CreateArmObject(self):
        arcparts = 100
        a1= [0.0, 0.0, 0.0]
        a2= [0.0, 0.0, 0.0]
        staticArcPoints = vtk.vtkPoints()
        staticArcPoints.SetNumberOfPoints(arcparts*4)
        height = 0.0
        for i in range(0, arcparts):
            staticArcPoints.InsertPoint(i, 0, 0, height)
            height += 0.1
        cells2 = vtk.vtkCellArray()
        for j in range(0, arcparts-1):
            cells2.InsertNextCell(2)		
            cells2.InsertCellPoint(j)
            cells2.InsertCellPoint(j+1)
        self.armObject = vtk.vtkPolyData()
        self.armObject.SetLines(cells2)	
        self.armObject.SetPoints(staticArcPoints)
        self.armObject.Update()
        

        path = os.path.join(self.appdir, "Shaders")
        
        self.xray_shader = vtk.vtkProperty()
        self.xray_shader.SetAmbient(1.0)
        self.xray_shader.LoadMaterial(os.path.join(path, "GLSLxray.xml"))
        self.xray_shader.ShadingOn()        
        a = self.gui.ren2.GetActiveCamera().GetViewTransformMatrix()
        campos = self.gui.ren2.GetActiveCamera().GetPosition()
        self.xray_shader.AddShaderVariableFloat ("camPos", campos[0], campos[1], campos[2])
        self.xray_shader.AddShaderVariableFloat ("camproj1", a.GetElement(0,0), a.GetElement(1,0), a.GetElement(2,0), a.GetElement(3,0))
        self.xray_shader.AddShaderVariableFloat ("camproj2", a.GetElement(0,1), a.GetElement(1,1), a.GetElement(2,1), a.GetElement(3,1))
        self.xray_shader.AddShaderVariableFloat ("camproj3", a.GetElement(0,2), a.GetElement(1,2), a.GetElement(2,2), a.GetElement(3,2))
        self.xray_shader.AddShaderVariableFloat ("camproj4", a.GetElement(0,3), a.GetElement(1,3), a.GetElement(2,3), a.GetElement(3,3))
        
        self.exoblue_shader = vtk.vtkProperty()
        self.exoblue_shader.SetAmbient(1.0)
        self.exoblue_shader.SetColor(0.3,0.3,1.0)
        self.exoblue_shader.LoadMaterial(os.path.join(path, "GLSLTwistThis.xml"))
        self.exoblue_shader.AddShaderVariableFloat("Rate", 1.0)
        self.exoblue_shader.AddShaderVariableFloat("Amplitude", 0.0)
        self.exoblue_shader.AddShaderVariableFloat("alphamultiplier", 1.0)
        
        self.exoyellow_shader = vtk.vtkProperty()
        self.exoyellow_shader.SetAmbient(1.0)
        self.exoyellow_shader.SetColor(0.7,0.7,0.3)
        self.exoyellow_shader.LoadMaterial(os.path.join(path, "GLSLTwistThis.xml"))
        self.exoyellow_shader.AddShaderVariableFloat("Rate", 1.0)
        self.exoyellow_shader.AddShaderVariableFloat("Amplitude", 0.0)
        self.exoyellow_shader.AddShaderVariableFloat("alphamultiplier", 1.0)
        
        self.blue_shader = vtk.vtkProperty()
        self.blue_shader.SetAmbient(1.0)
        self.blue_shader.SetColor(0.3,0.3,1.0)#1.0,1.0,0.3)#0.3,0.3,1.0)
        self.blue_shader.LoadMaterial(os.path.join(path, "GLSLBlueMe.xml"))
        self.blue_shader.AddShaderVariableFloat("alphamultiplier", 1.0)
        self.blue_shader.AddShaderVariableFloat ("camPos", campos[0], campos[1], campos[2])
        self.blue_shader.AddShaderVariableFloat ("camproj1", a.GetElement(0,0), a.GetElement(1,0), a.GetElement(2,0), a.GetElement(3,0))
        self.blue_shader.AddShaderVariableFloat ("camproj2", a.GetElement(0,1), a.GetElement(1,1), a.GetElement(2,1), a.GetElement(3,1))
        self.blue_shader.AddShaderVariableFloat ("camproj3", a.GetElement(0,2), a.GetElement(1,2), a.GetElement(2,2), a.GetElement(3,2))
        self.blue_shader.AddShaderVariableFloat ("camproj4", a.GetElement(0,3), a.GetElement(1,3), a.GetElement(2,3), a.GetElement(3,3))
        
        self.yellow_shader = vtk.vtkProperty()
        self.yellow_shader.SetAmbient(1.0)
        self.yellow_shader.SetColor(0.7,0.7,0.3)
        self.yellow_shader.LoadMaterial(os.path.join(path, "GLSLBlueMe.xml"))
        self.yellow_shader.AddShaderVariableFloat("alphamultiplier", 1.0)
        self.yellow_shader.AddShaderVariableFloat ("camPos", campos[0], campos[1], campos[2])
        self.yellow_shader.AddShaderVariableFloat ("camproj1", a.GetElement(0,0), a.GetElement(1,0), a.GetElement(2,0), a.GetElement(3,0))
        self.yellow_shader.AddShaderVariableFloat ("camproj2", a.GetElement(0,1), a.GetElement(1,1), a.GetElement(2,1), a.GetElement(3,1))
        self.yellow_shader.AddShaderVariableFloat ("camproj3", a.GetElement(0,2), a.GetElement(1,2), a.GetElement(2,2), a.GetElement(3,2))
        self.yellow_shader.AddShaderVariableFloat ("camproj4", a.GetElement(0,3), a.GetElement(1,3), a.GetElement(2,3), a.GetElement(3,3))
        
        #SPLINES:
        self.bluespline_shader = vtk.vtkProperty()
        self.bluespline_shader.SetAmbient(1.0)
        self.bluespline_shader.SetColor(0.3,0.3,1.0)
        self.bluespline_shader.LoadMaterial(os.path.join(path, "GLSLJustColor.xml"))
        self.bluespline_shader.AddShaderVariableFloat("alphamultiplier", 1.0)
        self.yellowspline_shader = vtk.vtkProperty()
        self.yellowspline_shader.SetAmbient(1.0)
        self.yellowspline_shader.SetColor(1.0,1.0,0.3)
        self.yellowspline_shader.LoadMaterial(os.path.join(path, "GLSLJustColor.xml"))
        self.yellowspline_shader.AddShaderVariableFloat("alphamultiplier", 1.0)


    
            
    def Render(self):
        self.gui.renwini1.Render()
        self.gui.renwini2.Render()
        self.gui.renwini3.Render()
        

        
    def SmoothObject(self, obj):
        Smooth = vtk.vtkSmoothPolyDataFilter()
        Smooth.SetInput(obj)        
        Smooth.SetNumberOfIterations(50)
        Smooth.SetRelaxationFactor(0.01)
        return Smooth.GetOutput()
        
    def RecalcNormals(self, obj):
        Normals = vtk.vtkPolyDataNormals()
        Normals.SetInput(obj)
        Normals.SetFeatureAngle(60.0)
        return Normals.GetOutput()
     
    def ClearGlyphs(self):
        for i in self.glyphlist:
            self.gui.ren3.RemoveActor(i)
        self.glyphlist[:] = []
                    
        
    def ClearData(self, whichone=2):
        if whichone==0 or whichone==2:
            for i in self.splineCollection:
                self.Filters.HideSpline(i)
        if whichone==1 or whichone==2:
            for i in self.splineReferences:
                self.Filters.HideSpline(i)
        
        #~ self.ClearGlyphs()
                
        for i in self.Filters.jointAngleCollection:
            if whichone==0 or whichone==2:
                i['min1'] =None
                i['max1'] =None
                if i['widget'] != None:
                    i['widget'].SetWidgetRange1(90.0, 90.0)
            if whichone==1 or whichone==2:
                if i['widget'] != None:
                    i['widget'].SetWidgetRange2(90.0, 90.0)
                i['min2'] =None
                i['max2'] =None
            if whichone==2:
                i['filter_min'] = None
                i['filter_max'] = None
                if i['widget'] != None:
                    i['widget'].SetfilterRangeMin(90.0)
                    i['widget'].SetfilterRangeMax(90.0)

        self.gui.widgets.UpdateWidgets()

        if whichone==0 or whichone==2:
            self.splineCollection[:] = []
            self.kdpoints = vtk.vtkPoints()
            self.kdvertices = vtk.vtkCellArray()
            self.kdpolydata = vtk.vtkPolyData()
        if whichone==1 or whichone==2:
            self.splineReferences[:] = []
            self.kdpoints2 = vtk.vtkPoints()
            self.kdvertices2 = vtk.vtkCellArray()        
            self.kdpolydata2 = vtk.vtkPolyData()        
        self.kdtree_modified = True
        
        self.cellpicker.InitializePickList()
        self.gui.Render()
        
            
    def RegenerateSplineObjects(self):
        self.ClearGlyphs()
        for spline in self.splineCollection:
            profileActor = self.CreateSplineObject(spline['stance'],\
                                        updatewidget = False, referenceSpline=False)
            spline['splineActor'].SetMapper(profileActor.GetMapper())
            
        for spline in self.splineReferences:
            profileActor = self.CreateSplineObject(spline['stance'],\
                                        updatewidget = False, referenceSpline=True)
            spline['splineActor'].SetMapper(profileActor.GetMapper())

        
        
    def CorrectBLsForStaticElements(self, blPos, useStableJointIndex = True):
        self.angleCalculations.SetupCoordinateSystems(blPos)
        if self.stablejointindex >= 0 or not useStableJointIndex:
            transfThorax = self.CorrectSpine()
        else:
            transfThorax = vtk.vtkTransform()
        if self.stablejointindex >= 1 or not useStableJointIndex:
            transfClavicle = self.CorrectOXYZ(self.angleCalculations.OXYZThorax, self.angleCalculations.Right_OXYZClavicle)
        else:
            transfClavicle = vtk.vtkTransform()
        
        claviclevector = (numpy.array(blPos[6])-numpy.array(blPos[4]))
        p1 = claviclevector.tolist()
        vtk.vtkMath().Normalize(p1)
        norm = lambda x: numpy.sqrt(numpy.square(claviclevector).sum())                    
        current_clavicle_length = norm(claviclevector)
        length_difference = self.clavicle_length - current_clavicle_length
        transfClavicle.Translate((numpy.array(p1) * length_difference).tolist())

        if self.stablejointindex >= 2 or not useStableJointIndex:
            transfScapula = self.CorrectOXYZ(self.angleCalculations.Right_OXYZClavicle, self.angleCalculations.Right_OXYZScapula)
        else:
            transfScapula = vtk.vtkTransform()
        if self.stablejointindex >= 3 or not useStableJointIndex:
            transfHumerus = self.CorrectOXYZ(self.angleCalculations.Right_OXYZScapula, self.angleCalculations.Right_OXYZHumerus)
        else:
            transfHumerus = vtk.vtkTransform()
        
        humerusvector = (((numpy.array(blPos[10])+numpy.array(blPos[11]))/2)-numpy.array(blPos[24]))
        p2 = humerusvector.tolist()
        vtk.vtkMath().Normalize(p2)
        norm = lambda x: numpy.sqrt(numpy.square(humerusvector).sum())                    
        current_humerus_length = norm(humerusvector)
        length_difference2 = self.humerus_length - current_humerus_length
        transfHumerus.Translate((numpy.array(p2) * length_difference2).tolist())      
        
        
        if self.stablejointindex >= 4 or not useStableJointIndex:
            transfForearm = self.CorrectOXYZ(self.angleCalculations.Right_OXYZHumerus, self.angleCalculations.Right_OXYZForearm)
        else:
            transfForearm = vtk.vtkTransform()
            
        new_blPos = blPos[:]
        
        #SPINE
        spine = [0, 1, 2, 3, 4]
        for pt in spine:
            new_blPos[pt] = transfThorax.TransformFloatPoint(blPos[pt])

        #CLAVICLE
        clavicle = [6]
        tmp = self.ApplyCorrectionTransform(transfClavicle, blPos[4], blPos[6]) 
        new_blPos[6] = transfThorax.TransformFloatPoint(tmp)
        
        #SCAPULA
        scapula = [5, 7, 8, 9, 24]
        for pt in scapula:
            tmp1 = self.ApplyCorrectionTransform(transfScapula, blPos[6], blPos[pt])
            tmp2 = self.ApplyCorrectionTransform(transfClavicle, blPos[4], tmp1)
            tmp3 = transfThorax.TransformFloatPoint(tmp2)
            new_blPos[pt] = tmp3

        # HUMERUS
        humerus = [10, 11]
        for pt in humerus:
            tmp1 = self.ApplyCorrectionTransform(transfHumerus, blPos[24], blPos[pt])
            tmp2 = self.ApplyCorrectionTransform(transfScapula, blPos[6], tmp1)
            tmp3 = self.ApplyCorrectionTransform(transfClavicle, blPos[4], tmp2)
            tmp4 = transfThorax.TransformFloatPoint(tmp3)
            new_blPos[pt] = tmp4

        # FOREARM
        forearm = [12, 13]
        for pt in forearm:
            tmp1 = self.ApplyCorrectionTransform(transfForearm, self.angleCalculations.Right_midELEM, blPos[pt])
            tmp2 = self.ApplyCorrectionTransform(transfHumerus, blPos[24], tmp1)
            tmp3 = self.ApplyCorrectionTransform(transfScapula, blPos[6], tmp2)    
            tmp4 = self.ApplyCorrectionTransform(transfClavicle, blPos[4], tmp3)
            tmp5 = transfThorax.TransformFloatPoint(tmp4)
            new_blPos[pt] = tmp5
            
        return new_blPos
        
    def GenerateArmObject(self, em, el, gh, exoindex, color):
        newarm = vtk.vtkPolyData()
        newarm.DeepCopy(self.armObject)                        
        #~ map = vtk.vtkPolyDataMapper()
        map = vtktudoss.vtktudExtendedOpenGLPolyDataMapper()        
        map.SetInput(newarm)
        shaderactor = vtk.vtkActor()
        shaderactor.SetMapper(map)
        if color==1:
            shaderactor.SetProperty(self.exoblue_shader)
        else:
            shaderactor.SetProperty(self.exoyellow_shader)
        #~ shaderactor.GetProperty().SetColor(color[0], color[1], color[2])
        #~ shaderactor.GetProperty().SetDiffuseColor(
        shaderactor.GetProperty().ShadingOn()
        
        vfl = vtk.vtkFloatArray()
        vfl.SetNumberOfComponents(3)    
        rated_exo = float(exoindex)/180
        for i in range(0,100):
            vfl.InsertTuple3(i, rated_exo, color,0)
        newarm.GetPointData().SetNormals(vfl)
        newarm.Update()
        
        mPoints = vtk.vtkPoints()
        rPoints = vtk.vtkPoints()
        mPoints.InsertPoint(0, [-0.5,0.0,10.0])
        rPoints.InsertPoint(0, em)
        mPoints.InsertPoint(1, [0.5,0.0,10.0])
        rPoints.InsertPoint(1, el)
        mPoints.InsertPoint(2, [0.0, 0.0, 0.0])
        rPoints.InsertPoint(2, gh)
        transformer = vtk.vtkLandmarkTransform()
        transformer.SetSourceLandmarks(mPoints)
        transformer.SetTargetLandmarks(rPoints)
        transformer.Update()        
        shaderactor.SetUserTransform(transformer)
        
        
        
        return shaderactor
        
        
        
    def ShowPose(self, blPos_spline, type, stance = None, showbones = True, shadedArm = False): #type -> 0 Collection 1 Reference 2 mean
        poseAppend = vtk.vtkAppendPolyData()
        
        #~ new_blPos = self.CorrectBLsForStaticElements(blPos_spline)
        #~ blPos = self.angleCalculations.SetupCoordinateSystems(new_blPos)
        blPos = self.CorrectBLsForStaticElements(blPos_spline)
        midPXT8 = self._root.baseCalc.Average2Points(blPos[0], blPos[3])
        midIJC7 = self._root.baseCalc.Average2Points(blPos[1], blPos[2])
        Right_midELEM = self._root.baseCalc.Average2Points(blPos[10], blPos[11])
        
        if showbones:
            self.SetBones(blPos)
            
        poseAppend = vtk.vtkAppendPolyData()
        #~ poseAppend.AddInput(self.PoseVis_add(self.angleCalculations.midPXT8, self.angleCalculations.midIJC7, "spine"))
        poseAppend.AddInput(self.PoseVis_add(midPXT8, midIJC7, "spine"))
        poseAppend.AddInput(self.PoseVis_add(blPos[4], blPos[6], "clavicle"))
        poseAppend.AddInput(self.PoseVis_add(blPos[7], blPos[8], "scapula"))
        poseAppend.AddInput(self.PoseVis_add(blPos[8], blPos[9], "scapula"))
        poseAppend.AddInput(self.PoseVis_add(blPos[9], blPos[7], "scapula"))
        #~ poseAppend.AddInput(self.PoseVis_add(self.angleCalculations.Right_midELEM, blPos[13], "arm")) 
        poseAppend.AddInput(self.PoseVis_add(Right_midELEM, blPos[13], "arm")) 
        
        
        
        if shadedArm:
            if type == 0:
                ShadedArmActor = self.GenerateArmObject(blPos[10], blPos[11], blPos[24], stance[11], color=1) #[0.3, 0.3, 1.0]
            else:
                ShadedArmActor = self.GenerateArmObject(blPos[10], blPos[11], blPos[24], stance[11], color=0) # [1.0, 1.0, 0.3]
        
        poseMapper = vtktudoss.vtktudExtendedOpenGLPolyDataMapper()
        poseActor = vtk.vtkActor()
        poseActor.SetMapper(poseMapper)
        #~ poseMapper = vtk.vtkPolyDataMapper()
        poseMapper.SetInput(poseAppend.GetOutput())
        #~ poseMapper.ScalarVisibilityOn()
        #~ poseMapper.SetUseLookupTableScalarRange(1)
        #~ poseMapper.SetScalarModeToUsePointData()
        
        #~ tmppoly = vtk.vtkPolyData()
        #~ tmppoly.DeepCopy(poseAppend.GetOutput())
        #~ tmppoly.Update()
        #~ poseMapper.SetInput(tmppoly) #tmppoly)#
        
        #~ vfl = vtk.vtkFloatArray()
        #~ vfl.SetNumberOfComponents(3)    
        #~ alpha = 0.5
        #~ for i in range(0,14):
            #~ vfl.InsertTuple3(i, alpha, alpha, alpha)
        #~ tmppoly.GetPointData().SetNormals(vfl)
        #~ tmppoly.Update()
        #~ tmppoly.GetPointData().SetNormals(vfl)
        #~ tmppoly.Update()
        
        poseMapper.Update()
        
        if type == 0:
            #~ poseMapper.SetLookupTable(self.colorscales.dynamic_generic)
            #poseActor.GetProperty().SetOpacity(0.05)
            poseActor.SetProperty(self.blue_shader)
            poseActor.GetProperty().ShadingOn()
        elif type ==1:
            #~ poseMapper.SetLookupTable(self.colorscales.dynamic_generic2)
            #~ poseActor.GetProperty().SetOpacity(0.05)
            poseActor.SetProperty(self.yellow_shader)
            poseActor.GetProperty().ShadingOn()
        else:
            #~ poseMapper.SetLookupTable(self.colorscales.dynamic_generic3)
            poseActor.GetProperty().SetOpacity(1.0)        
        
        
        if shadedArm:
            return [poseActor, ShadedArmActor]
        else:
            return poseActor
    
    def ApplyCorrectionTransform(self, transform, origin, point):
        point2 = self.baseCalc.SubtractPoints(point, origin)
        point2 = transform.TransformPoint(point2)
        point2 = self.baseCalc.AddPoints(origin, point2)  
        return point2
        
    def CorrectOXYZ(self, parentOXYZ, childOXYZ):
        old_OXYQ =               [[0,0,0],\
                                        childOXYZ[1],\
                                        childOXYZ[2],\
                                        childOXYZ[3]]
        target_OXYQ =               [[0,0,0],\
                                        parentOXYZ[1],\
                                        parentOXYZ[2],\
                                        parentOXYZ[3]]
        return self.angleCalculations.MatchCoordinateSystemAtoB(old_OXYQ, target_OXYQ)

    def CorrectSpine(self):
        currentOXYZThorax_GLOBAL = [self.angleCalculations.OXYZThorax[0],\
                                                      self.baseCalc.AddPoints(self.angleCalculations.OXYZThorax[0], self.angleCalculations.OXYZThorax[1]),\
                                                      self.baseCalc.AddPoints(self.angleCalculations.OXYZThorax[0], self.angleCalculations.OXYZThorax[2]),\
                                                      self.baseCalc.AddPoints(self.angleCalculations.OXYZThorax[0], self.angleCalculations.OXYZThorax[3])]        
        O = [0,0,0]
        X = self.baseCalc.AddPoints(O, [1,0,0])
        Y = self.baseCalc.AddPoints(O, [0,1,0])
        Z = self.baseCalc.AddPoints(O, [0,0,1])
        goalOXYZThorax_GLOBAL = [O, X, Y, Z]        
        return self.angleCalculations.MatchCoordinateSystemAtoB(currentOXYZThorax_GLOBAL, goalOXYZThorax_GLOBAL)

    def RegeneratePoses(self):
        self.kdpoints = vtk.vtkPoints()
        self.kdvertices = vtk.vtkCellArray()
        for i in range(0, len(self.splineCollection)):
            actorcollection = self.ShowPose(self.splineCollection[i]['blPos'], 0,\
                        self.splineCollection[i]['stance'], showbones = False, shadedArm=True)
            self.splineCollection[i]['poseActor'].SetMapper(actorcollection[0].GetMapper())
            self.splineCollection[i]['poseActor'].GetMapper().Update()
            self.splineCollection[i]['shadedArm'].SetMapper(actorcollection[1].GetMapper()) 
            self.splineCollection[i]['shadedArm'].SetUserTransform(actorcollection[1].GetUserTransform())
            self.splineCollection[i]['shadedArm'].GetMapper().Update()
            self.kdpoints.InsertNextPoint(self.splineCollection[i]['poseActor'].GetMapper()\
                                                        .GetInput().GetPoints().GetPoint(11))
            self.kdvertices.InsertNextCell(1)
            self.kdvertices.InsertCellPoint(self.kdpoints.GetNumberOfPoints()-1)
        self.kdpolydata = vtk.vtkPolyData()
        self.kdpolydata.SetPoints(self.kdpoints)
        self.kdpolydata.SetVerts(self.kdvertices)
            
        self.kdpoints2 = vtk.vtkPoints()
        self.kdvertices2 = vtk.vtkCellArray()
        for i in range(0, len(self.splineReferences)):
            actorcollection = self.ShowPose(self.splineReferences[i]['blPos'], 1,\
                        self.splineReferences[i]['stance'], showbones = False, shadedArm=True)
            self.splineReferences[i]['poseActor'].SetMapper(actorcollection[0].GetMapper())
            self.splineReferences[i]['poseActor'].GetMapper().Update()
            self.splineReferences[i]['shadedArm'].SetMapper(actorcollection[1].GetMapper()) 
            self.splineReferences[i]['shadedArm'].SetUserTransform(actorcollection[1].GetUserTransform())
            self.splineReferences[i]['shadedArm'].GetMapper().Update()
            self.kdpoints2.InsertNextPoint(self.splineReferences[i]['poseActor'].GetMapper()\
                                                        .GetInput().GetPoints().GetPoint(11))
            self.kdvertices2.InsertNextCell(1)
            self.kdvertices2.InsertCellPoint(self.kdpoints2.GetNumberOfPoints()-1)
        self.kdpolydata2 = vtk.vtkPolyData()
        self.kdpolydata2.SetPoints(self.kdpoints2)
        self.kdpolydata2.SetVerts(self.kdvertices2)
        
        self.Filters.RestartFiltering()
        self.Filters.RestartFilteringReferences()        

        

    def PoseVis_add(self, point1, point2, part):
        polys = vtk.vtkCellArray()
        points = vtk.vtkPoints()
        lijntje = vtk.vtkPolyData()
        points.InsertPoint(0, point1)
        points.InsertPoint(1, point2)
        polys.InsertNextCell(2)
        polys.InsertCellPoint(0)
        polys.InsertCellPoint(1)
        lijntje.SetPoints(points)
        lijntje.SetLines(polys)    

        if part == "arm":
            vfl = vtk.vtkFloatArray()
            vfl.SetNumberOfComponents(3)    
            alpha = 0.2
            vfl.InsertTuple3(0, alpha, alpha, alpha)
            vfl.InsertTuple3(1, alpha, alpha, alpha)
            lijntje.GetPointData().SetNormals(vfl)
            lijntje.Update()
            return lijntje

        elif part == "scapula":
            vfl = vtk.vtkFloatArray()
            vfl.SetNumberOfComponents(3)    
            alpha = 0.04
            vfl.InsertTuple3(0, alpha, alpha, alpha)
            vfl.InsertTuple3(1, alpha, alpha, alpha)
            lijntje.GetPointData().SetNormals(vfl)
            lijntje.Update()
            return lijntje

        elif part == "spine":
            vfl = vtk.vtkFloatArray()
            vfl.SetNumberOfComponents(3)    
            alpha = 1.0
            vfl.InsertTuple3(0, alpha, alpha, alpha)
            vfl.InsertTuple3(1, alpha, alpha, alpha)
            lijntje.GetPointData().SetNormals(vfl)
            lijntje.Update()
            return lijntje
            
        elif part == "clavicle":
            vfl = vtk.vtkFloatArray()
            vfl.SetNumberOfComponents(3)    
            alpha = 0.04
            vfl.InsertTuple3(0, alpha, alpha, alpha)
            vfl.InsertTuple3(1, alpha, alpha, alpha)
            lijntje.GetPointData().SetNormals(vfl)
            lijntje.Update()
            return lijntje
            
        
    def OldPoseVis_add(self, point1, point2, part):
        polys = vtk.vtkCellArray()
        points = vtk.vtkPoints()
        lijntje = vtk.vtkPolyData()
        points.InsertPoint(0, point1)
        points.InsertPoint(1, point2)
        polys.InsertNextCell(2)
        polys.InsertCellPoint(0)
        polys.InsertCellPoint(1)
        lijntje.SetPoints(points)
        lijntje.SetLines(polys)    

        if part == "arm":
            poseColors = vtk.vtkFloatArray() 
            poseColors.SetName("poseColors")  
            lijntje.GetPointData().AddArray(poseColors)
            lijntje.GetPointData().SetActiveScalars("poseColors")                        
            poseColors.InsertTuple1(0, 0)
            poseColors.InsertTuple1(1, 0)
            return lijntje

        elif part == "scapula":
            poseColors = vtk.vtkFloatArray() 
            poseColors.SetName("poseColors")  
            lijntje.GetPointData().AddArray(poseColors)
            lijntje.GetPointData().SetActiveScalars("poseColors")                        
            poseColors.InsertTuple1(0, 1)
            poseColors.InsertTuple1(1, 1)
            return lijntje

        elif part == "spine":
            poseColors = vtk.vtkFloatArray() 
            poseColors.SetName("poseColors")  
            lijntje.GetPointData().AddArray(poseColors)
            lijntje.GetPointData().SetActiveScalars("poseColors")                        
            poseColors.InsertTuple1(0, 3)
            poseColors.InsertTuple1(1, 3)
            return lijntje
            
        elif part == "clavicle":
            poseColors = vtk.vtkFloatArray() 
            poseColors.SetName("poseColors")  
            lijntje.GetPointData().AddArray(poseColors)
            lijntje.GetPointData().SetActiveScalars("poseColors")                        
            poseColors.InsertTuple1(0, 2)
            poseColors.InsertTuple1(1, 2)
            return lijntje



            
            
            
            

        

    def StanceRated(self, stance):
        stance_rated = []
        for i in stance:
            stance_rated = stance_rated + [-1 + float(i)/180]
        
        return stance_rated
        
    def CreateSplineObject(self, stance, updatewidget = False, referenceSpline=False):
        # Generate SplineObject
        aSplineX = vtk.vtkCardinalSpline()
        aSplineY = vtk.vtkCardinalSpline()
        aSplineZ = vtk.vtkCardinalSpline()
        # Generate random (pivot) points and add the corresponding
        # coordinates to the splines.
        # aSplineX will interpolate the x values of the points
        # aSplineY will interpolate the y values of the points
        # aSplineZ will interpolate the z values of the points
        inputPoints = vtk.vtkPoints()
        x_count = 0
        numberOfInputPoints = len(self.Filters.plottedAngles)
        
        stance_usedonly = []
        for i in self.Filters.plottedAngles:
            stance_usedonly.append(stance[i])
        stance_rated = self.StanceRated(stance_usedonly) #conversion to range [0, 2]
            
        for i in range(0, numberOfInputPoints):                    
            y = stance_rated[i]            
            x_count = x_count + 1
            x = x_count
            z = 0    
            aSplineX.AddPoint(i, x)
            aSplineY.AddPoint(i, y)
            aSplineZ.AddPoint(i, z)
            inputPoints.InsertPoint(i, x, y, z)

        profileData = vtk.vtkPolyData()
        if len(self.Filters.plottedAngles)>1:
            # Generate the polyline for the spline.
            points = vtk.vtkPoints()            
            # Number of points on the spline
            if self.gui.editorpanel.radio_btn_6.GetValue():
                numberOfOutputPoints = len(self.Filters.plottedAngles) #linear
            else:
                numberOfOutputPoints = len(self.Filters.plottedAngles) * 14
            # Interpolate x, y and z by using the three spline filters and
            # create new points
            for i in range(0, numberOfOutputPoints):
                t = (numberOfInputPoints-1.0)/(numberOfOutputPoints-1.0)*i
                points.InsertPoint(i, aSplineX.Evaluate(t), aSplineY.Evaluate(t),
                                   aSplineZ.Evaluate(t))
                
            # Create the polyline.
            lines = vtk.vtkCellArray()
            lines.InsertNextCell(numberOfOutputPoints)
            for i in range(0, numberOfOutputPoints):
                lines.InsertCellPoint(i)
            profileData.SetPoints(points)
            profileData.SetLines(lines)
        
        #~ profileMapper = vtk.vtkPolyDataMapper()
        profileMapper = vtktudoss.vtktudExtendedOpenGLPolyDataMapper()
        profileMapper.SetInput(profileData)
        profileActor = vtk.vtkActor()
        profileActor.SetMapper(profileMapper)
        #~ profileActor.GetProperty().SetOpacity(0.05)
        if referenceSpline == True:
            #set yellow
            profileActor.SetProperty(self.yellowspline_shader)
            profileActor.GetProperty().ShadingOn()
        else:
            #set blue
            profileActor.SetProperty(self.bluespline_shader)
            profileActor.GetProperty().ShadingOn()
        self.CreateGlyphs(inputPoints, referenceSpline)
        profileMapper.Update()
        return profileActor
        
        

    def CreateGlyphs(self, inputPoints, referenceSpline):
        # Create a polydata to be glyphed.
        inputData = vtk.vtkPolyData()
        inputData.SetPoints(inputPoints)
        # Use sphere as glyph source.
        balls = vtk.vtkSphereSource()
        balls.SetRadius(.01)
        balls.SetPhiResolution(4)
        balls.SetThetaResolution(4)
        glyphPoints = vtk.vtkGlyph3D()
        glyphPoints.SetInput(inputData)
        glyphPoints.SetSource(balls.GetOutput())
        glyphMapper = vtk.vtkPolyDataMapper()
        glyphMapper.SetInputConnection(glyphPoints.GetOutputPort())
        glyph = vtk.vtkActor()
        glyph.SetMapper(glyphMapper)
        if referenceSpline:
            glyph.GetProperty().SetDiffuseColor(banana)
            glyph.SetPosition(0.02, 0.0, 0.0)
        else:
            glyph.GetProperty().SetDiffuseColor(blue_light )
            glyph.SetPosition(-0.02, 0.0, 0.0)
        glyph.GetProperty().SetSpecular(.3)
        glyph.GetProperty().SetSpecularPower(30)
        self.gui.ren3.AddActor(glyph)
        self.glyphlist.append(glyph)
        

        

        
    def AddSpline(self, stance, blpositions, updatewidget = False, referenceSpline = False):
        #Update Range Indicators (Not visually)
        if referenceSpline:
            range_min = 'min2' 
            range_max = 'max2' 
        else:
            range_min = 'min1' 
            range_max = 'max1'
        for i in range(0, len(self.Filters.jointAngleCollection)):
            if self.Filters.jointAngleCollection[i]['max1'] == None:
                self.Filters.jointAngleCollection[i]['filter_min'] = stance[i]-1
                self.Filters.jointAngleCollection[i]['filter_max'] = stance[i]
                self.Filters.jointAngleCollection[i][range_min] = stance[i]-1
                self.Filters.jointAngleCollection[i][range_max] = stance[i]                
            elif stance[i] > self.Filters.jointAngleCollection[i][range_max] or self.Filters.jointAngleCollection[i][range_max] == None:
                if not referenceSpline:
                    if self.Filters.jointAngleCollection[i]['filter_max'] == self.Filters.jointAngleCollection[i]['max1']:
                        self.Filters.jointAngleCollection[i]['filter_max'] = stance[i]
                self.Filters.jointAngleCollection[i][range_max] = stance[i]
            elif stance[i] < self.Filters.jointAngleCollection[i][range_min] or self.Filters.jointAngleCollection[i][range_min] == None:
                if not referenceSpline:
                    if self.Filters.jointAngleCollection[i]['filter_min'] == self.Filters.jointAngleCollection[i]['min1']:
                        self.Filters.jointAngleCollection[i]['filter_min'] = stance[i]
                self.Filters.jointAngleCollection[i][range_min] = stance[i]            
            
        if updatewidget == True:
            self.gui.widgets.UpdateWidgets()
        
        profileActor = self.CreateSplineObject(stance, updatewidget, referenceSpline)        
        
        if self.baseSkeleton == -1:
            self.ResetSkeleton(blpositions)
            self.GeneratePhysique([0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0])
        
        spline = {'stance' : stance,
                      'visible': False,
                        'splineActor' : profileActor,
                        'poseActor': None,
                        'shadedArm': None,
                        'reference': referenceSpline,
                        'blPos': blpositions}
        
        if referenceSpline == True:
            poseActorCollection = self.ShowPose(blpositions, 1, stance, showbones = (not referenceSpline and self.parent.parent!=None), shadedArm = True)
        else:
            poseActorCollection = self.ShowPose(blpositions, 0, stance, showbones = (not referenceSpline and self.parent.parent!=None), shadedArm = True)
        
        poseActor = poseActorCollection[0]        
        shadedArm = poseActorCollection[1]
        spline['poseActor'] = poseActor
        spline['shadedArm'] = shadedArm
        
        if referenceSpline == True:
            splineNr = len(self.splineReferences)
        else:
            splineNr = len(self.splineCollection)
        visible = self.Filters.FilterSingleSpline(spline, referenceSpline, splineNr)
        
        spline['visible'] = visible
        if referenceSpline == True:
            self.splineReferences.append(spline)
            self.kdpoints2.InsertNextPoint(poseActor.GetMapper()\
                                                        .GetInput().GetPoints().GetPoint(11))
            self.kdvertices2.InsertNextCell(1)
            self.kdvertices2.InsertCellPoint(self.kdpoints2.GetNumberOfPoints()-1)
        else:
            self.splineCollection.append(spline)
            self.kdpoints.InsertNextPoint(poseActor.GetMapper()\
                                                        .GetInput().GetPoints().GetPoint(11))
            self.kdvertices.InsertNextCell(1)
            self.kdvertices.InsertCellPoint(self.kdpoints.GetNumberOfPoints()-1)
        self.kdtree_modified = True
        #~ for i in range(0, len(self.gui.sequenceplotters)):
            #~ self.gui.UpdateSequencePlot(i, self.gui.sequenceplotters[i].whichdata)
                    
        
    def AddSkeleton(self):
        # 86 thorax: 0 1 2 3 4 14
        # 85 hand:11 12 13
        # 83 84 forearm: 10 11 12 13 
        # 82 humerus: 10 11 24 
        #81 schouderblad: 5 6 7 8 9 
        #80 clavicle: 0 14 4 6

        appdir = self.ReadRootDirectory()
        bonelist = [80, 81, 82, 83, 84, 85, 86]
        self.bone_landmarks = [[0, 4, 6], [5,6,7,8,9],\
                                           [10, 11, 24], [10, 11, 12, 13],\
                                           [10, 11, 12, 13], [11, 12,13], [0,1,2,3,4]] #14? andere clavicle
        self.bone_actorList = []
        self.bone_mapperList = []
        self.bone_mapperListNormal = []
        self.bone_readerList = []
        for i in range(0, len(bonelist)):
            #~ if bonelist[i] != 81 and bonelist[i] != 82: 
            fname = os.path.join(appdir, 'Skeleton', ('Bone_' + str(bonelist[i]) + '.vtk'))
            #~ else:
                #~ if bonelist[i] == 81:
                    #~ ##~ fname = os.path.join(appdir, 'Skeleton', ('scap_emery.vtk'))
                    #~ fname = os.path.join(appdir, 'Skeleton', ('scap_emery_env.vtk'))
                #~ else:
                    #~ fname = os.path.join(appdir, 'Skeleton', ('hum_emery.vtk'))
                    
            self.bone_actorList.append(vtk.vtkActor())
            #~ self.bone_mapperList.append(vtk.vtkPolyDataMapper())
            self.bone_mapperList.append(vtktudoss.vtktudExtendedOpenGLPolyDataMapper())
            self.bone_mapperListNormal.append(vtk.vtkPolyDataMapper())
            self.bone_readerList.append(vtk.vtkPolyDataReader())
            
            self.bone_readerList[i].SetFileName(fname)
            self.bone_readerList[i].OpenVTKFile()
            obj = self.RecalcNormals(self.SmoothObject(\
                        self.bone_readerList[i].GetOutput()))

        
            #~ self.bone_mapperList[i].SetInput(self.bone_readerList[i].GetOutput())
            self.bone_mapperList[i].SetInput(obj)
            self.bone_mapperListNormal[i].SetInput(obj)
            self.bone_actorList[i].SetMapper(self.bone_mapperListNormal[i])
            #~ self.bone_actorList[i].SetProperty(self.xray_shader)
            self.gui.ren2.AddActor(self.bone_actorList[i])            
            self.bone_actorList[i].GetProperty().SetColor(1, .97, .85)
            self.bone_readerList[i].CloseVTKFile()

                
                


    def SetBones(self, blPos):        
        for i in range(0, len(self.bone_actorList)):
            
            linkedBLs = self.bone_landmarks[i]
            
            mPoints = vtk.vtkPoints()
            rPoints = vtk.vtkPoints()
            for j in range(0, len(linkedBLs)):
                mPoints.InsertPoint(j, self.BLmeshList[linkedBLs[j]])
                rPoints.InsertPoint(j, blPos[linkedBLs[j]])
            transformer = vtk.vtkLandmarkTransform()
            transformer.SetSourceLandmarks(mPoints)
            transformer.SetTargetLandmarks(rPoints)
            
            self.bone_actorList[i].SetUserTransform(transformer)
        if self.pointWidget.GetEnabled():
            self.pointWidget.SetPosition(blPos[13]) 
        
        
    def PlayShader(self):
        #~ while True:
        #~ try:
            #~ wx.SafeYield(onlyIfNeeded = True)                        
        #~ except AssertionError:
            #~ pass
        #~ addedrate = -1 + float(self.lastStance[11])/180
        self.shaderrate = self.shaderrate + 1
        self.pulserate = self.pulserate + 0.3
        self.exoblue_shader.AddShaderVariableFloat ("Rate", self.shaderrate)
        self.exoyellow_shader.AddShaderVariableFloat ("Rate", self.shaderrate)
        #~ print self.shaderrate
        
        #~ try:
            #~ wx.SafeYield(onlyIfNeeded = False)                        
        #~ except AssertionError:
            #~ pass
        #~ time.sleep(0.01)
            
    def InitializePointWidget(self):
        if self.pointwidgetinitialized == False:
            
            self.pointWidget = vtk.vtkPointWidget()        
            self.pointWidget.PlaceWidget(-50, 50, -100, 100, 0, 100)
            self.pointWidget.AllOff()        
            self.pointWidget.KeyPressActivationOff()
            self.pointWidget.GetProperty().SetColor(1,1,1)
            self.pointWidget.SetHandleSize(10)
            self.pointWidget.SetInteractor(self.gui.renwini2)
            #~ self.pointWidget.EnabledOn()
            self.pointwidgetinitialized = True
        
            self.pointWidget.AddObserver("InteractionEvent", self.ProbeData)
        #~ pointWidget.AddObserver("StartInteractionEvent", BeginInteraction)        
        #~ self.pointWidget.AddObserver("EndInteractionEvent", self.ProbeData)
            

    def ProbeData(self, event, bla):
        if self.kdtree_modified == True:
            self.kdpolydata = vtk.vtkPolyData()
            self.kdpolydata.SetPoints(self.kdpoints)
            self.kdpolydata.SetVerts(self.kdvertices)
            self.kdpolydata2 = vtk.vtkPolyData()
            self.kdpolydata2.SetPoints(self.kdpoints2)
            self.kdpolydata2.SetVerts(self.kdvertices2)
            self.IK.Buildobbtree()
            self.kdtree_modified = False
            
        self.Filters.inversekinematicsOn = True
        
        newpos = self.pointWidget.GetPosition() 
        oldpos = self.angleCalculations.blPos[13]
        #~ testpos1 = self.angleCalculations.blPos[13]#[-1.06, -58.3, 21.6]
        #~ testpos2 = [-1, -32, 49]

        laststancefound = self.IK.Queryobbtree(newpos, self.baseCalc.Average2Points(\
                                                                    self.angleCalculations.blPos[10], self.angleCalculations.blPos[11]))
        # INVERSE KINEMATICS:
        if laststancefound == None:
            newstance = self.IK.Run(oldpos, newpos)
        else:
            newstance = laststancefound
        
        widgetposition = self.pointWidget.GetPosition()
        if newstance == -1:
            pass
            #~ self.pointWidget.SetPosition(oldpos) 
        else:
            
            self.GeneratePhysique(newstance, showbones=True)
            self.pointWidget.SetPosition(widgetposition) 
            self.gui.SetSliders(newstance)
        self.gui.Render(2)
        self.gui.Render(3)            
        
    def ResetSkeleton(self, blPos): 
        claviclevector = (numpy.array(blPos[6])-numpy.array(blPos[4]))
        norm = lambda x: numpy.sqrt(numpy.square(claviclevector).sum())                
        self.clavicle_length = norm(claviclevector)
        humerusvector = (((numpy.array(blPos[10])+numpy.array(blPos[11]))/2)-numpy.array(blPos[24]))
        norm = lambda x: numpy.sqrt(numpy.square(humerusvector).sum())                
        self.humerus_length = norm(humerusvector)
        new_blPos = self.CorrectBLsForStaticElements( blPos, useStableJointIndex = False)        
        self.baseSkeleton = new_blPos        
        self.SetBones(new_blPos)
        self.pointWidget.SetPosition(new_blPos[13])
        
    def GeneratePhysique(self, stance, showbones = False, returnBLpos = False):
        #Given a stance (series of angles), we reconstruct a skeleton pose that meets the stance angle requirements.
        # Ideally, no full stance is required and we can for example reconstruct single angles.        

        blPos = self.angleCalculations.SetupCoordinateSystems(self.baseSkeleton)

        self.lastStance = stance
        new_blPos = blPos[:]
        
        #SPINE
        t2_thorax = vtk.vtkTransform()
        t2_thorax.RotateWXYZ(stance[1], self.angleCalculations.OXYZThorax[2])
        t2_thorax.Update()
        basicAxis = t2_thorax.TransformPoint(self.angleCalculations.OXYZThorax[1])        
        transfThorax = vtk.vtkTransform()        
        #~ transfThorax.RotateWXYZ(-stance[0], basicAxis)
        #~ transfThorax.RotateWXYZ(stance[2], self.angleCalculations.OXYZThorax[2])
        transfThorax.Update()
        
        #CLAVICLE
        t2_clavicle = vtk.vtkTransform()
        t2_clavicle.RotateWXYZ(stance[4], self.angleCalculations.Right_OXYZClavicle[2])
        t2_clavicle.Update()
        basicAxis = t2_clavicle.TransformPoint(self.angleCalculations.Right_OXYZClavicle[1])        
        transfClavicle = vtk.vtkTransform()        
        transfClavicle.RotateWXYZ(-stance[3], basicAxis)
        transfClavicle.RotateWXYZ(stance[5], self.angleCalculations.Right_OXYZClavicle[2])
        transfClavicle.Update()
        
        #SCAPULA
        t2_scapula = vtk.vtkTransform()
        t2_scapula.RotateWXYZ(stance[7], self.angleCalculations.Right_OXYZScapula[2])
        t2_scapula.Update()
        basicAxis = t2_scapula.TransformPoint(self.angleCalculations.Right_OXYZScapula[1])        
        transfScapula = vtk.vtkTransform()        
        transfScapula.RotateWXYZ(-stance[6], basicAxis)
        transfScapula.RotateWXYZ(stance[8], self.angleCalculations.Right_OXYZScapula[2])
        transfScapula.Update()
        
        #HUMERUS
        t2_humerus = vtk.vtkTransform()
        t2_humerus.RotateWXYZ(stance[10], self.angleCalculations.Right_OXYZHumerus[2])
        t2_humerus.Update()
        basicAxis = t2_humerus.TransformPoint(self.angleCalculations.Right_OXYZHumerus[1])        
        transfHumerus = vtk.vtkTransform()                
        transfHumerus.RotateWXYZ(-stance[9], basicAxis)
        transfHumerus.RotateWXYZ(stance[11], self.angleCalculations.Right_OXYZHumerus[2])
        transfHumerus.Update()
        
        #FOREARM
        t2_forearm = vtk.vtkTransform()
        t2_forearm.RotateWXYZ(stance[13], self.angleCalculations.Right_OXYZForearm[2])
        t2_forearm.Update()
        basicAxis = t2_forearm.TransformPoint(self.angleCalculations.Right_OXYZForearm[1])        
        transfForearm = vtk.vtkTransform()        
        #~ transfForearm.RotateWXYZ(stance[12], basicAxis)
        #~ transfForearm.RotateWXYZ(stance[14], self.angleCalculations.Right_OXYZForearm[2])
        transfForearm.RotateWXYZ(-stance[12], basicAxis)
        transfForearm.RotateWXYZ(stance[14], self.angleCalculations.Right_OXYZForearm[2])
        transfForearm.Update()

        
        
        #SPINE
        spine = [0, 1, 2, 3, 4]
        for pt in spine:
            new_blPos[pt] = transfThorax.TransformFloatPoint(blPos[pt])

        #CLAVICLE
        clavicle = [6]
        tmp = self.ApplyCorrectionTransform(transfClavicle, blPos[4], blPos[6]) 
        new_blPos[6] = transfThorax.TransformFloatPoint(tmp)
        
        #SCAPULA
        scapula = [5, 7, 8, 9, 24]
        for pt in scapula:
            tmp1 = self.ApplyCorrectionTransform(transfScapula, blPos[6], blPos[pt])
            tmp2 = self.ApplyCorrectionTransform(transfClavicle, blPos[4], tmp1)
            tmp3 = transfThorax.TransformFloatPoint(tmp2)
            new_blPos[pt] = tmp3

        # HUMERUS
        humerus = [10, 11]
        for pt in humerus:
            tmp1 = self.ApplyCorrectionTransform(transfHumerus, blPos[24], blPos[pt])
            tmp2 = self.ApplyCorrectionTransform(transfScapula, blPos[6], tmp1)
            tmp3 = self.ApplyCorrectionTransform(transfClavicle, blPos[4], tmp2)
            tmp4 = transfThorax.TransformFloatPoint(tmp3)
            new_blPos[pt] = tmp4

        # FOREARM
        forearm = [12, 13]
        for pt in forearm:
            tmp1 = self.ApplyCorrectionTransform(transfForearm, self.angleCalculations.Right_midELEM, blPos[pt])
            tmp2 = self.ApplyCorrectionTransform(transfHumerus, blPos[24], tmp1)
            tmp3 = self.ApplyCorrectionTransform(transfScapula, blPos[6], tmp2)    
            tmp4 = self.ApplyCorrectionTransform(transfClavicle, blPos[4], tmp3)
            tmp5 = transfThorax.TransformFloatPoint(tmp4)
            new_blPos[pt] = tmp5    
        
        if returnBLpos:
            return new_blPos
        else:
            poseActor = self.ShowPose(new_blPos, type = 2, showbones = showbones) #type = mean        
            return poseActor
        
  


    def MedianMeanSpline(self, mean = True):
        try:
            self.gui.ren3.RemoveActor(self.statisticsActor)
        except AttributeError:
            pass
        try:
            self.gui.ren2.RemoveActor(self.customPose)
        except AttributeError:
            pass

        stancelist = []
        stancelength = len(self.splineCollection[0]['stance'])
        for i in range(0,len(self.splineCollection)):
            totalstance = []
            if self.splineCollection[i]['visible'] == True:
                for j in range(0, stancelength):
                    totalstance.append(self.splineCollection[i]['stance'][j])
                stancelist.append(tuple(totalstance))

        if stancelist == []:
            return -1
            
        meanlist = []
        meanpluslist = []
        meanminlist = []
        medianlist = []
        lowerlist = []
        upperlist = []
        for i in range(0, stancelength):
            stats = statistics.stats(numpy.array(stancelist)[:,i].tolist(), confidence_interval = 0.25)
            meanlist.append(stats[0])
            meanminlist.append(stats[0]-stats[2])
            meanpluslist.append(stats[0]+stats[2])            
            medianlist.append(stats[1])
            lowerlist.append(stats[5])
            upperlist.append(stats[6])
            #~ lowerlist.append(stats[1]-stats[5])
            #~ upperlist.append(stats[1]+stats[5])

        if mean:
            lowerConfActor = self.GeneratePhysique(meanminlist)        
            medianActor = self.GeneratePhysique(meanlist, showbones = True)
            upperConfActor = self.GeneratePhysique(meanpluslist)        
            lowerConfActor.GetMapper().Update()
            medianActor.GetMapper().Update()
            upperConfActor.GetMapper().Update() 
        else:
            lowerConfActor = self.GeneratePhysique(lowerlist)        
            medianActor = self.GeneratePhysique(medianlist, showbones = True)
            upperConfActor = self.GeneratePhysique(upperlist)        
            lowerConfActor.GetMapper().Update()
            medianActor.GetMapper().Update()
            upperConfActor.GetMapper().Update()        
        
        #~ self.StoreImage()
        return 1
        
        obj1 = lowerConfActor.GetMapper().GetInput()
        obj2 = medianActor.GetMapper().GetInput()
        obj3 = upperConfActor.GetMapper().GetInput()
        
        
        
        # Generate the polyline for the spline.
        points = vtk.vtkPoints()
        profileData = vtk.vtkPolyData()
        # Number of points on the spline
        numberOfOutputPoints = 14# * 3 #14 punten per pose, 3 poses
              
        ptlist1 = obj1.GetPoints()
        ptlist2 = obj2.GetPoints()
        ptlist3 = obj3.GetPoints()
        interpolatedDensity = 31
        splineColors = vtk.vtkFloatArray() 
        splineColors.SetName("statistics")     
        for i in range(0, numberOfOutputPoints):
            SplineX = vtk.vtkCardinalSpline()
            SplineY = vtk.vtkCardinalSpline()
            SplineZ = vtk.vtkCardinalSpline()
            SplineX.AddPoint(0, ptlist1.GetPoint(i)[0])      #
            SplineX.AddPoint(1, ptlist2.GetPoint(i)[0])
            SplineX.AddPoint(2, ptlist3.GetPoint(i)[0])
            SplineY.AddPoint(0, ptlist1.GetPoint(i)[1])    
            SplineY.AddPoint(1, ptlist2.GetPoint(i)[1])      #
            SplineY.AddPoint(2, ptlist3.GetPoint(i)[1])  
            SplineZ.AddPoint(0, ptlist1.GetPoint(i)[2])
            SplineZ.AddPoint(1, ptlist2.GetPoint(i)[2])
            SplineZ.AddPoint(2, ptlist3.GetPoint(i)[2])      #
                    
            for j in range(0, interpolatedDensity):
                t = (j*1.0) / (0.5 * interpolatedDensity)
                points.InsertNextPoint(SplineX.Evaluate(t), SplineY.Evaluate(t), SplineZ.Evaluate(t))
                splineColors.InsertTuple1(i*interpolatedDensity + j, 60*abs(j - (0.5 * interpolatedDensity))/(0.5 * interpolatedDensity))
                
        surface = vtk.vtkCellArray()        
        for i in range(0, numberOfOutputPoints-1):
            if i not in [1, 3, 9, 11]:
                for j in range(0, interpolatedDensity-1):
                    surface.InsertNextCell(4)
                    surface.InsertCellPoint(i * interpolatedDensity + j)
                    surface.InsertCellPoint(i * interpolatedDensity + j + 1)
                    surface.InsertCellPoint((i+1) * interpolatedDensity + j + 1)
                    surface.InsertCellPoint((i+1) * interpolatedDensity + j)

        profileData = vtk.vtkPolyData()
        profileData.SetPoints(points)
        profileData.SetPolys(surface)
        
        statisticsMapper = vtk.vtkPolyDataMapper()  
        statisticsMapper.SetInput(profileData)
        statisticsMapper.ScalarVisibilityOn()
        statisticsMapper.GetInput().GetPointData().AddArray(splineColors)
        statisticsMapper.GetInput().GetPointData().SetActiveScalars("statistics")        
        statisticsMapper.SetUseLookupTableScalarRange(1)        
        statisticsMapper.SetScalarModeToUsePointData()
        ##~ statisticsMapper.SetScalarMaterialModeToAmbientAndDiffuse()
        statisticsMapper.SetLookupTable(self.colorscales.LUT_Statistics)
        self.customPose.SetMapper(statisticsMapper)
        ##~ self.customPose.GetProperty().SetAmbient(0.5)
        ##~ self.customPose.GetProperty().SetDiffuse(0.5)
        #~ self.gui.ren2.AddActor(self.customPose)
        self.gui.Render(2)
        
        
        
        
        if mean:
            stdspline1 = self.CreateSplineObject(meanminlist).GetMapper().GetInput()
            meanspline = self.CreateSplineObject(meanlist).GetMapper().GetInput()
            stdspline2 = self.CreateSplineObject(meanpluslist).GetMapper().GetInput()
        else:
            stdspline1 = self.CreateSplineObject(upperlist).GetMapper().GetInput()
            meanspline = self.CreateSplineObject(medianlist).GetMapper().GetInput()
            stdspline2 = self.CreateSplineObject(lowerlist).GetMapper().GetInput()
        
        # Generate the polyline for the spline.
        points = vtk.vtkPoints()
        profileData = vtk.vtkPolyData()
        # Number of points on the spline
        numberOfOutputPoints = 50
        # Interpolate x, y and z by using the three spline filters and
        # create new points
        ptlist = stdspline1[0].GetPoints()
        for i in range(0, numberOfOutputPoints):
            points.InsertNextPoint(ptlist.GetPoint(i))
        ptlist = meanspline[0].GetPoints()
        for i in range(0, numberOfOutputPoints):
            points.InsertNextPoint(ptlist.GetPoint(i))
        ptlist = stdspline2[0].GetPoints()
        for i in range(0, numberOfOutputPoints):
            points.InsertNextPoint(ptlist.GetPoint(i))
        
        splineColors = vtk.vtkFloatArray() 
        splineColors.SetName("statistics")     
        for i in range(0, numberOfOutputPoints):
            splineColors.InsertTuple1(i, 60)
        for i in range(0, numberOfOutputPoints):
            splineColors.InsertTuple1(i+numberOfOutputPoints, 0)
        for i in range(0, numberOfOutputPoints):
            splineColors.InsertTuple1(i+2*numberOfOutputPoints, 60)
        
        #~ # Create the polyline.
        surface = vtk.vtkCellArray()        
        for i in range(0, numberOfOutputPoints-1):
            surface.InsertNextCell(4)
            surface.InsertCellPoint(i)
            surface.InsertCellPoint(i + 1)
            surface.InsertCellPoint(i + numberOfOutputPoints + 1)
            surface.InsertCellPoint(i + numberOfOutputPoints)
            surface.InsertNextCell(4)
            surface.InsertCellPoint(i + numberOfOutputPoints + 1)
            surface.InsertCellPoint(i + numberOfOutputPoints)            
            surface.InsertCellPoint(i + 2*numberOfOutputPoints)
            surface.InsertCellPoint(i + 2*numberOfOutputPoints + 1)

        profileData = vtk.vtkPolyData()
        profileData.SetPoints(points)
        profileData.SetPolys(surface)
        statisticsMapper = vtk.vtkPolyDataMapper()  
        statisticsMapper.SetInput(profileData)
        statisticsMapper.ScalarVisibilityOn()
        statisticsMapper.GetInput().GetPointData().AddArray(splineColors)
        statisticsMapper.GetInput().GetPointData().SetActiveScalars("statistics")        
        statisticsMapper.SetUseLookupTableScalarRange(1)
        statisticsMapper.SetScalarModeToUsePointData()
        statisticsMapper.SetLookupTable(self.colorscales.LUT_Statistics)        
        self.statisticsActor.SetMapper(statisticsMapper)
        self.statisticsActor.SetPosition(0,0,-1)
        self.gui.ren3.AddActor(self.statisticsActor)
        self.gui.Render(3)
        
        
        
    def StoreImage(self, whichone=1):
        pngWriter = vtk.vtkPNGWriter()
        w2i = vtk.vtkWindowToImageFilter()
        w2i.SetInput(self.gui.renwini2.GetRenderWindow())
        #~ if whichone==2:
            #~ w2i.SetInput(self.main.renwini2.GetRenderWindow())
        #~ else:
            #~ w2i.SetInput(self.main.renwini.GetRenderWindow())        
        pngWriter.SetInput(w2i.GetOutput())
        w2i.Modified()
        self.imagenumber = self.imagenumber+1
        pngWriter.SetFileName('Screenshots/image%04d.png' % self.imagenumber)
        pngWriter.Write()
        

    def ExportSplines(self, whichone):
        filename = ""
        dlg = wx.FileDialog(self.gui, message="Save splines", defaultDir=os.getcwd(), defaultFile="",
        wildcard="txt file (*.txt)|*.txt", style=wx.SAVE | wx.CHANGE_DIR)
        if dlg.ShowModal() == wx.ID_OK:
            filename = dlg.GetPath()            
        dlg.Destroy()
        
        exportfile = open(filename, 'wb', 1)
        
        if whichone==0:
            for i in range (0, len (self.splineCollection)):
                
                if self.Filters.FilterSingleSpline_WithoutVisualisation(\
                                                self.splineCollection[i], whichone, i):
                    exportfile.write(str(self.splineCollection[i]['stance'])+"\n")
                    exportfile.write(str(self.splineCollection[i]['blPos'])+"\n")
        else:
            for i in range (0, len (self.splineReferences)):
                if self.Filters.FilterSingleSpline_WithoutVisualisation(\
                                                self.splineReferences[i], whichone, i):
                    exportfile.write(str(self.splineReferences[i]['stance'])+"\n")
                    exportfile.write(str(self.splineReferences[i]['blPos'])+"\n")
        exportfile.close()
        
        return os.path.split(filename)[1]
        


        
    def ReadRootDirectory(self):
        if hasattr(sys, 'frozen') and sys.frozen:
            appdir, exe = os.path.split(sys.executable)
        else:
            dirname = os.path.dirname(sys.argv[0])
            if dirname and dirname != os.curdir:
                appdir = dirname
            else:
                appdir = os.getcwd()
        return appdir


    def ActivateCellpicker(self):           
        # Create a cell picker.            
        if (self.cellpickercreated==True):
            self.cellpickercreated = True    
        else:
            self.cellpickercreated = True
            self.cellpicker = vtk.vtkPointPicker()
            self.cellpicker.SetTolerance(0.0025)
            print "Cellpicker tolerance: ", self.cellpicker.GetTolerance()
            
            self.cellpicker.InitializePickList()
            self.cellpicker.PickFromListOn()   

    def ActivateAngleRollOver(self, event):
        #print "activate"
        if self.rollover_activated == False:
            self.ActivateCellpicker()                  
            self.rollover_activated = True
            
    def DeactivateAngleRollOver(self, event):
        #print "deactivate"
        if self.rollover_activated == True:
            self.gui.renwini1.RemoveObserver(self.lbpe)
            self.rollover_activated = False

    def AngleRollOver(self, event, bla):
        x, y = self.gui.renwini3.GetEventPosition()
        self.cellpicker.Pick(x, y, 0, self.gui.ren3)
       
        
        if self.cellpicker.GetPointId() < 0:
            pass
        else:
            #print self.cellpicker.GetPointId()
            act = self.cellpicker.GetActor()
            #~ activescalars = act.GetMapper().GetInput().GetPointData().GetActiveScalars()
            #~ print act.GetMapper().GetInput().GetPointData().GetScalars()#"splinecolors_reference")            
            #~ print act.GetMapper().GetInput().GetPointData().GetScalars(splineColors)
            
            
            try:
                self.gui.ren3.RemoveActor(self.TubeActor)
            except AttributeError:
                pass
            self.TubeActor = vtk.vtkActor()
            profileTubes = vtk.vtkTubeFilter()
            profileTubes.SetNumberOfSides(3)
            profileTubes.SetInput(act.GetMapper().GetInput())
            profileTubes.SetRadius(.005)
            self.TubeMapper = vtk.vtkPolyDataMapper()            
            self.TubeActor.SetMapper(self.TubeMapper)
            self.TubeActor.GetProperty().SetAmbient(1.0)
            self.TubeMapper.SetInput(profileTubes.GetOutput())
            self.gui.ren3.AddActor(self.TubeActor)
            
            if not act.GetMapper().GetInput().GetPointData().GetScalars("splinecolors_reference"):
                for i in range(0, len(self.splineCollection)):
                    if self.splineCollection[i]['splineActor'] == act:
                        blPos = self.splineCollection[i]['blPos']
                        new_blPos = self.CorrectBLsForStaticElements(blPos)
                        self.lastStance = self.splineCollection[i]['stance']
                        
                        try:
                            self.gui.ren2.RemoveActor(self.PoseActor9)
                        except AttributeError:
                            pass
                        self.PoseActor9.SetMapper(act.GetMapper())

                        act = self.ShowPose(new_blPos, type = 2)
                        self.gui.SetSliders(self.splineCollection[i]['stance'])
            else:
                for i in range(0, len(self.splineReferences)):
                    if self.splineReferences[i]['splineActor'] == act:
                        blPos = self.splineReferences[i]['blPos']
                        new_blPos = self.CorrectBLsForStaticElements(blPos)
                        self.lastStance = self.splineReferences[i]['stance']
                        
                        try:
                            self.gui.ren2.RemoveActor(self.PoseActor9)
                        except AttributeError:
                            pass
                        self.PoseActor9.SetMapper(act.GetMapper())

                        act = self.ShowPose(new_blPos, type = 2)
                        self.gui.SetSliders(self.splineReferences[i]['stance'])
                        
            self.gui.Render()

  
class MyApp(wx.App):
    def OnInit(self):
        patternviewer = PatternViewer(None, None)
        
        #~ patternviewer.Show()
        self.SetTopWindow(patternviewer.gui)
        #frame.AddSpline([120, 100, 50, 140, 10, 12, 90, 180, 180], ['None', 'None', 'None', 'None'], None)
        patternviewer.ImportSplines()
        patternviewer.ImportSplines2()

        #~ frame.SwitchClavicleTransform()
        #~ frame.GenerateReachObject()
        return 1

if __name__ == "__main__":
    app = MyApp(0)
    app.MainLoop()